package com.gitee.wsl.mathematics.complex.quaternion

import com.gitee.wsl.ext.array.DoubleArray
import com.gitee.wsl.ext.array.FloatArray
import com.gitee.wsl.io.dataproxy.DataProxy
import com.gitee.wsl.io.dataproxy.asProxy
import com.gitee.wsl.mathematics.coordinate.d3.Coordinate3D
import com.gitee.wsl.mathematics.geometry.d3.ext.norm
import com.gitee.wsl.mathematics.vector.ext.dot
import com.gitee.wsl.mathematics.complex.quaternion.Quat.Companion.rotationAroundQuat
import com.gitee.wsl.mathematics.vector.vec2.MutableVec2
import com.gitee.wsl.mathematics.vector.vec3.Vec3
import com.gitee.wsl.mathematics.vector.vec3.MutableVec3
import com.gitee.wsl.mathematics.vector.vec3.Vec3D
import kotlin.math.PI
import kotlin.math.asin
import kotlin.math.cos
import kotlin.math.sin


/**
 * A class representing an effective rotation of a vector(x, y, z) by theta, where w = cos(theta/2).
 */
class Quat(override val proxy: DataProxy<Double>):
    QuaternionD<Quat> {
    constructor(x: Double=0.0,
                y: Double=0.0,
                z: Double=0.0,
                w: Double=0.0):this(DoubleArray(x,y,z,w).asProxy())
    constructor(a: Number) : this(a.toDouble(), a.toDouble(), a.toDouble(), a.toDouble())
    constructor(x: Number, y: Number, z: Number, w: Number) : this(x.toDouble(), y.toDouble(), z.toDouble(), w.toDouble())
    constructor(euler: Vec3D) : this() {
        val halfA1 = euler.y * 0.5
        val halfA2 = euler.x * 0.5
        val halfA3 = euler.z * 0.5
        val cosA1 = cos(halfA1)
        val sinA1 = sin(halfA1)
        val cosA2 = cos(halfA2)
        val sinA2 = sin(halfA2)
        val cosA3 = cos(halfA3)
        val sinA3 = sin(halfA3)
        x = sinA1 * cosA2 * sinA3 + cosA1 * sinA2 * cosA3
        y = sinA1 * cosA2 * cosA3 - cosA1 * sinA2 * sinA3
        z = cosA1 * cosA2 * sinA3 - sinA1 * sinA2 * cosA3
        w = sinA1 * sinA2 * sinA3 + cosA1 * cosA2 * cosA3
    }

    constructor(angle: Number, axis: Vec3D) : this(0.0, 0.0, 0.0, 0.0) {
        val dAngle = angle.toDouble()
        val sin = sin(dAngle / 2.0)
        val nAxis = axis.normalized
        x = nAxis.x * sin
        y = nAxis.y * sin
        z = nAxis.z * sin
        w = cos(dAngle / 2.0)
    }

    override val self: Quat
        get() = this

    override fun create(x: Double, y: Double, z: Double, w: Double) = Quat(x,y,z,w)

//    override val euler: Vec3
//        get() {
//            val sinrcosp = 2 * (w * x + y * z)
//            val cosrsinp = 1 - 2 * (x * x + y * y)
//            val sinp = 2 * (w * y - z * x)
//            val sinycosp = 2 * (w * z + x * y)
//            val cosysinp = 1 - 2 * (y * y + z * z)
//            return Vec3(
//                atan2(sinrcosp, cosrsinp),
//                if (abs(sinp) >= 1) PI * 0.5 * sign(sinp) else asin(sinp),
//                atan2(sinycosp, cosysinp)
//            )
//        }

    override fun createVec2(a: Double, b: Double) = MutableVec2(a,b)

    override fun createVec3(a: Double, b: Double, c: Double)= MutableVec3(a,b,c)

    companion object {
        val ZERO get() = Quat(0.0, 0.0, 0.0, 0.0)
        val ONE get() = Quat(1.0, 1.0, 1.0, 1.0)
        val identity get() = Quat(0.0, 0.0, 0.0, 1.0)
        val negativeInfinity get() = Quat(Double.NEGATIVE_INFINITY)
        val positiveInfinity get() = Quat(Double.POSITIVE_INFINITY)

        fun rotationAroundQuat(x: Double, y: Double, z: Double, angle: Double): Quat {
            val s = sin(angle * 0.5) / norm(x, y, z)
            val w = cos(angle * 0.5)
            return Quat(s * x, s * y, s * z, w)
        }
    }

    override operator fun times(other: Quat) = Quat(
        w * other.x + x * other.w + y * other.z - z * other.y,
        w * other.y + y * other.w + z * other.x - x * other.z,
        w * other.z + z * other.w + x * other.y - y * other.x,
        w * other.w - x * other.x - y * other.y - z * other.z
    )

    /*operator fun times(point: Vec3D): Vec3 {
        val x = x * 2.0
        val y = y * 2.0
        val z = z * 2.0
        val xx = this.x * x
        val yy = this.y * y
        val zz = this.z * z
        val xy = this.x * y
        val xz = this.x * z
        val yz = this.y * z
        val wx = w * x
        val wy = w * y
        val wz = w * z
        return Vec3(
            (1.0 - (yy + zz)) * point.x + (xy - wz) * point.y + (xz + wy),
            (xy + wz) * point.x + (1.0 - (xx + zz)) * point.y + (yz - wx),
            (xz - wy) * point.x + (yz + wx) * point.y + (1.0 - (xx + yy))
        ) * point.z
    }*/

/*
    operator fun times(point: Vec3F): Vec3f {
        val x = x * 2.0
        val y = y * 2.0
        val z = z * 2.0
        val xx = this.x * x
        val yy = this.y * y
        val zz = this.z * z
        val xy = this.x * y
        val xz = this.x * z
        val yz = this.y * z
        val wx = w * x
        val wy = w * y
        val wz = w * z
        return Vec3f(
            (1.0 - (yy + zz)) * point.x + (xy - wz) * point.y + (xz + wy),
            (xy + wz) * point.x + (1.0 - (xx + zz)) * point.y + (yz - wx),
            (xz - wy) * point.x + (yz + wx) * point.y + (1.0 - (xx + yy))
        ) * point.z
    }
*/

    //infix fun dot(other: QuaternionD) = x * other.x + y * other.y + z * other.z + w * other.w

    /** The angle between this quaternion and [other] in **radians**. */
//    override infix fun angleTo(other: Quat): Double {
//        val dot = dot(other)
//        return if (dot > 1.0) 0.0 else acos(min(abs(dot), 1.0)) * 2.0
//    }

    override fun toString() = "{ $x; $y; $z; $w }"
    override fun equals(other: Any?) = other is Quat && dot(other) > 1
    override fun hashCode() = 31 * (31 * (31 * x.hashCode() + y.hashCode()) + z.hashCode()) + w.hashCode()
}

fun Coordinate3D<*>.toRotationAroundQuat(angle: Double): Quat = rotationAroundQuat(x, y, z, angle)





